#ifndef VL_GEOM_HPP
#define VL_GEOM_HPP

#include <algorithm>

namespace vl
{

enum intersect_result
{
	disjoint = 0,
	point_intersect,
	segment_intersect
};

template<typename VecT>
bool
in_segment( VecT const& P, VecT const& S0, VecT const& S1)
{
    if (S0.x != S1.x)
    {    // S is not vertical
        if (S0.x <= P.x && P.x <= S1.x)
            return true;
        if (S0.x >= P.x && P.x >= S1.x)
            return true;
    }
    else
    {
        if (S0.y <= P.y && P.y <= S1.y)
            return true;
        if (S0.y >= P.y && P.y >= S1.y)
            return true;
    }
    return false;
}

template<typename VecT>
intersect_result
segment_intersection_2d(
	VecT const& P0,
	VecT const& P1,
	VecT const& Q0,
	VecT const& Q1,
	VecT& O0, VecT& O1)
{
	typedef typename VecT::coord_type coord_type;
	coord_type const zero = coord_type();
	coord_type const one = coord_type(1);
	VecT u(P0, P1);
	VecT v(Q0, Q1);
	VecT w(Q0, P0);
	coord_type D = u.perp_dot(v);
	if(D == zero) // TODO: Make this a compare that works for floating-point too
	{
		if(u.perp_dot(w) != 0 || v.perp_dot(w) != 0)
		{
			return disjoint; // they are NOT collinear
		}

		// they are collinear or degenerate
		// check if they are degenerate points
		coord_type du = u.length_sqr();
		coord_type dv = v.length_sqr();

		if (du == zero && dv == zero)
		{
			if (P0 != Q0)         // they are distinct points
				return disjoint;

			O0 = P0;                // they are the same point
			return point_intersect;
		}
		if (du == zero)
		{                    // S1 is a single point
			if (!in_segment(P0, Q0, Q1))  // but is not in S2
				return disjoint;
			O0 = P0;
			return point_intersect;
		}
		if (dv == zero) {                    // S2 a single point
			if (!in_segment(Q0, P0, P1))  // but is not in S1
				return disjoint;
			O0 = Q0;
			return point_intersect;
		}
		// they are collinear segments - get overlap (or not)
		coord_type t0, t1;                   // endpoints of S1 in eqn for S2

		VecT w2(Q0, P1);
		if (v.x != zero)
		{
			t0 = w.x / v.x;
			t1 = w2.x / v.x;
		}
		else
		{
			t0 = w.y / v.y;
			t1 = w2.y / v.y;
		}

		if (t0 > t1)
		{                  // must have t0 smaller than t1
			std::swap(t0, t1);
		}
		if (t0 > one || t1 < zero)
		{
			return disjoint;     // NO overlap
		}
		if(t0 < zero) t0 = zero;
		if(t1 > one)  t1 = one;

		if (t0 == t1)
		{                 // intersect is a point
			O0 = Q0 + v * t0;
			return point_intersect;
		}

		// they overlap in a valid subsegment
		O0 = Q0 + v * t0;
		O1 = Q0 + v * t1;
		return segment_intersect;
	}
	else if(D < zero)
	{
		coord_type sI = v.perp_dot(w);
		if (sI > zero || sI < D)
			return disjoint;
		    
		coord_type tI = u.perp_dot(w);
		if (tI > zero || tI < D)
			return disjoint;

		O0 = P0 + (u * sI) / D;
		return point_intersect;
	}
	else
	{
		coord_type sI = v.perp_dot(w);
		if (sI < zero || sI > D)
			return disjoint;

		coord_type tI = u.perp_dot(w);
		if (tI < zero || tI > D)
			return disjoint;

		O0 = P0 + (u * sI) / D;
		return point_intersect;
	}
}

// Intersection test between thin segments (parallel segments never intersect).
// This is considerably simpler than the regular test.
// Result is in [disjoint, point_intersect]

template<typename VecT>
intersect_result
segment_intersection_thin_2d(
	VecT const& P0,
	VecT const& P1,
	VecT const& Q0,
	VecT const& Q1,
	VecT& O0)
{
	typedef typename VecT::coord_type coord_type;
	coord_type const zero = coord_type();

	VecT u(P0, P1);
	VecT v(Q0, Q1);
	VecT w(Q0, P0);
	coord_type D = u.perp_dot(v);
	if(D == zero) // TODO: Make this a compare that works for floating-point too
	{
		// Parallel segments never intersect
		return disjoint;
	}
	else if(D < zero)
	{
		coord_type sI = v.perp_dot(w);
		if (sI > zero || sI < D)
			return disjoint;
		    
		coord_type tI = u.perp_dot(w);
		if (tI > zero || tI < D)
			return disjoint;

		O0 = P0 + (u * sI) / D;
		return point_intersect;
	}
	else
	{
		coord_type sI = v.perp_dot(w);
		if (sI < zero || sI > D)
			return disjoint;

		coord_type tI = u.perp_dot(w);
		if (tI < zero || tI > D)
			return disjoint;

		O0 = P0 + (u * sI) / D;
		return point_intersect;
	}
}

}

#endif //VL_GEOM_HPP